/*
 * hf_inj.c
 *
 *  Created on: Apr 1, 2024
 *      Author: Yiwen Zhu
 */
#include "hf_inj.h"
#include "main.h"
#include "arm_math.h"
#include "math.h"
#include "i2c_as5600_dma.h"
#include "Motor_Control_Init.h"

#define HRTIM_ZERO_CMP	(0)
#define HRTIM_FULL_CMP	(DEF_SVM_TO_CMP)

SIX_PULES six_puls1={0};

static const char State[7][3] = {
		{0, 0, 0},	// 0 State V0/V7
		{1, 0, 0},	// 1
		{1, 1, 0},	// 2
		{0, 1, 0},	// 3
		{0, 1, 1},	// 4
		{0, 0, 1},	// 5
		{1, 0, 1},	// 6
};

void get_current_value(void){
	get_current_sample_real_value();
	six_puls1.Ia = clarke1.As;
	six_puls1.Ib = clarke1.Bs;
	six_puls1.Ic = clarke1.Cs;
}

// Sort for val and index
static void bubble_sort(float arr[], int index[], int n) {
    int i, j;
    float temp;
    for (i = 0; i < n-1; i++) {
        for (j = 0; j < n-i-1; j++) {
            if (arr[j] > arr[j+1]) {
                // Swap values
                temp = arr[j];
                arr[j] = arr[j+1];
                arr[j+1] = temp;
                // Swap indices
                int temp_index = index[j];
                index[j] = index[j+1];
                index[j+1] = temp_index;
            }
        }
    }
}

#define p1 		(1)
#define p2 		(5)
#define p3 		(6)
#define p4		(8)
#define wait	(25)

char init_angle_pull(void){
	static int Tick = 0;
	if(Tick++<20000){
		CTL_init_ang();
		return 0;
	}else{
		Tick = 0;
		return 1;
	}
}

// IPD Six-Pulse
char init_angle_meas(void){
	static int Init_Cnt = 0;
	int ind_temp[6]={0,1,2,3,4,5};
	int Ind;
	// 14 36 52
	get_current_value();
	G_Theta = GetAngle_Without_Track_DMA((uint16_t)M_NP)-DEF_ANG_OFFEST_DUAL_PI;
	while(G_Theta>DUAL_PI)	G_Theta -= DUAL_PI;
	while(G_Theta<0)	G_Theta += DUAL_PI;

	if(1){
		if(Init_Cnt >= p1 && Init_Cnt <= p2){
			Ind = 1;
		}else if(Init_Cnt >= p3 && Init_Cnt <= p4){
			Ind = 4;
		}else if(Init_Cnt >= wait+p1 && Init_Cnt <= wait+p2){
			Ind = 4;
		}else if(Init_Cnt >= wait+p3 && Init_Cnt <= wait+p4){
			Ind = 1;
		}else if(Init_Cnt >= wait*2+p1 && Init_Cnt <= wait*2+p2){
			Ind = 3;
		}else if(Init_Cnt >= wait*2+p3 && Init_Cnt <= wait*2+p4){
			Ind = 6;
		}else if(Init_Cnt >= wait*3+p1 && Init_Cnt <= wait*3+p3){
			Ind = 6;
		}else if(Init_Cnt >= wait*3+p3 && Init_Cnt <= wait*3+p4){
			Ind = 3;
		}else if(Init_Cnt >= wait*4+p1 && Init_Cnt <= wait*4+p2){
			Ind = 5;
		}else if(Init_Cnt >= wait*4+p3 && Init_Cnt <= wait*4+p4){
			Ind = 2;
		}else if(Init_Cnt >= wait*5+p1 && Init_Cnt <= wait*5+p2){
			Ind = 2;
		}else if(Init_Cnt >= wait*5+p3 && Init_Cnt <= wait*5+p4){
			Ind = 5;
		}else
			Ind = 0;

		switch(Init_Cnt){
			case (p3):
				six_puls1.Cur_buf[0] = six_puls1.Ia; break;	// A
			case (wait+p3):
				six_puls1.Cur_buf[1] = -six_puls1.Ia; break;// -A
			case (2*wait+p3):
				six_puls1.Cur_buf[2] = six_puls1.Ib; break;	// B
			case (3*wait+p3):
				six_puls1.Cur_buf[3] = -six_puls1.Ib; break;// -B
			case (4*wait+p3):
				six_puls1.Cur_buf[4] = six_puls1.Ic; break;	// C
			case (5*wait+p3):
				six_puls1.Cur_buf[5] = -six_puls1.Ic; break;// -C
		}


		if(Init_Cnt++>7*(wait+(p2-p1))){
			Init_Cnt = 0;

			bubble_sort(six_puls1.Cur_buf,ind_temp,6);
			six_puls1.Step = ind_temp[5];
			switch (six_puls1.Step){
				case 0: six_puls1.Theta = 0;break;	// 0 deg
				case 1: six_puls1.Theta = PI;break;	// 180 deg
				case 2: six_puls1.Theta = 0.6667f*PI;break;	// 120 deg
				case 3: six_puls1.Theta = 1.6667f*PI;break;	// 300 deg
				case 4: six_puls1.Theta = 1.3333f*PI;break;	// 240 deg
				case 5: six_puls1.Theta = 0.3333f*PI;break;	// 60 deg
			}
//			return 1;
		}
	}

	// If the switching state is 1，set a higher compare value
	if(State[Ind][0]==1)	six_puls1.Ta = HRTIM_FULL_CMP;
	else					six_puls1.Ta = HRTIM_ZERO_CMP;

	if(State[Ind][1]==1)	six_puls1.Tb = HRTIM_FULL_CMP;
	else					six_puls1.Tb = HRTIM_ZERO_CMP;

	if(State[Ind][2]==1)	six_puls1.Tc = HRTIM_FULL_CMP;
	else					six_puls1.Tc = HRTIM_ZERO_CMP;

	u16 temp[3];
	temp[0] = (u16)( ((six_puls1.Ta)*(DEF_SVM_TO_CMP)) -5); // DEF_SVM_TO_CMP
	temp[1] = (u16)( ((six_puls1.Tb)*(DEF_SVM_TO_CMP)) -5);
	temp[2] = (u16)( ((six_puls1.Tc)*(DEF_SVM_TO_CMP)) -5);
	__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1, temp[0]);    //修改比较值，修改占空比
	__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_2, temp[1]);    //修改比较值，修改占空比
	__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, temp[2]);    //修改比较值，修改占空比

	return 0;
}

void FHI_injection_task(void){
	if(hfi1.Inj_Sign == 1){
		hfi1.Uout = hfi1.Uinj;
		hfi1.Inj_Sign = -1;
	}
	else{
		hfi1.Uout = -hfi1.Uinj;
		hfi1.Inj_Sign = 1;
	}
}

void HFI_calc_task(void){
	float Delt_Ial_High,Delt_Ibe_High;

//	float Delta_Theta;
	// Low frequency current
	hfi1.Ial_Low = 0.5*(hfi1.Ial +hfi1.Ial_last);
	hfi1.Ibe_Low = 0.5*(hfi1.Ibe +hfi1.Ibe_last);
	// High frequency current
	hfi1.Ial_High = hfi1.Ial -hfi1.Ial_Low;//-hfi1.Ial_last;//
	hfi1.Ibe_High = hfi1.Ibe -hfi1.Ibe_Low;//-hfi1.Ibe_last;//
	// Delta high frequency current
	if(hfi1.Inj_Sign==1){
		Delt_Ial_High = hfi1.Ial_High;
		Delt_Ibe_High = hfi1.Ibe_High;
//		Delt_Ial_High = hfi1.Lpfal_last*(1-hfi1.LPF_alpha) + (hfi1.Ial_High) *hfi1.LPF_alpha;
//		Delt_Ibe_High = hfi1.Lpfbe_last*(1-hfi1.LPF_alpha) + (hfi1.Ibe_High) *hfi1.LPF_alpha;
	}
	else{
		Delt_Ial_High =  -hfi1.Ial_High;
		Delt_Ibe_High =  -hfi1.Ibe_High;
//		Delt_Ial_High = hfi1.Lpfal_last*(1-hfi1.LPF_alpha) - (hfi1.Ial_High) *hfi1.LPF_alpha;
//		Delt_Ibe_High = hfi1.Lpfbe_last*(1-hfi1.LPF_alpha) - (hfi1.Ibe_High) *hfi1.LPF_alpha;
	}

	if(six_puls1.Init_Flag==0){
//		if(hfi1.Theta-six_puls1.Theta >= 1.2f) hfi1.Offset = -PI;
//		else if(hfi1.Theta-six_puls1.Theta <= -1.2f) hfi1.Offset = +PI;
//		else  hfi1.Offset = 0;
		hfi1.Theta = six_puls1.Theta;
		hfi1.sin = arm_sin_f32(hfi1.Theta);
		hfi1.cos = arm_cos_f32(hfi1.Theta);
		six_puls1.Init_Flag = 1;
	}

//	 PLL
	float Delta_Theta;
	Delta_Theta = Delt_Ibe_High*hfi1.cos -Delt_Ial_High*hfi1.sin;
	hfi1.Up = hfi1.Kp*Delta_Theta;
	hfi1.Ui += hfi1.Ki*Delta_Theta;
	hfi1.Theta += hfi1.Up + hfi1.Ui;
	hfi1.sin = arm_sin_f32(hfi1.Theta);
	hfi1.cos = arm_cos_f32(hfi1.Theta);

	// Atan
//	hfi1.Theta = atan2f(Delt_Ibe_High,Delt_Ial_High) ; //


	hfi1.Theta +=  hfi1.Offset;	// -0.25*PI

	if(hfi1.Theta<0) hfi1.Theta += DUAL_PI;

	// record the data at k-1
	hfi1.Ial_last = hfi1.Ial;
	hfi1.Ibe_last = hfi1.Ibe;
	hfi1.Lpfal_last = Delt_Ial_High;
	hfi1.Lpfbe_last = Delt_Ibe_High;

	FHI_injection_task();
}


